#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import time  
import subprocess
import WSDiscovery 
from robot_pkg.msg import Person1



class camera_ip(object):

	def __init__(self):
		self.camera_ip=[]   #摄像机IP
		self.camera_ip1 = ""
		self.camera_ip2 = ""
		#print("搜索摄像机IP.....")
	def get_camera_ip(self)->bool:
		cmd = 'hostname -I'
		scope = subprocess.check_output(cmd, shell=True).decode('utf-8')
		wsd = WSDiscovery.WSDiscovery()
		wsd.start()
		ret = wsd.searchServices()
		for service in ret:
			get_ip = str(service.getXAddrs())
			get_types = str(service.getTypes())
			for ip_scope in scope.split():
				result = get_ip.find(ip_scope.split('.')[0] + '.' + ip_scope.split('.')[1])
				if result > 0 and get_types.find('onvif') > 0:
					string_result = get_ip[result:result+13]
					string_result = get_ip[result:].split('/')[0]
					string_result = string_result.split(':')

					#检查是否有效
					if string_result[0][0:9] != "192.168.1":
						continue
					#检查是否存在
					if string_result[0] in self.camera_ip:
						continue
					self.camera_ip.append(string_result[0])
		#print(self.camera_ip)
		wsd.stop()
		if (len(self.camera_ip) ==1):
			self.camera_ip1 = self.camera_ip[0]
			self.camera_ip2 = ""
		if (len(self.camera_ip) >=2):
			self.camera_ip1 = self.camera_ip[0]
			self.camera_ip2 = self.camera_ip[1]
		# if(len(self.camera_ip) >=2):
		# 	for index in range(len(self.camera_ip)):
		# 		self.camera_ip1 = self.camera_ip[0]
		# 		if(self.camera_ip1 != self.camera_ip[index]):
		# 			self.camera_ip2 = self.camera_ip[index]
		# 			if(self.camera_ip1[0:9] == "192.168.1" and self.camera_ip2[0:9] == "192.168.1"):
		# 				return True
		time.sleep(1)
		#return False
	def cameraIP_publisher(self):
		# ROS节点初始化
		rospy.init_node('cameraIP_publisher', anonymous=True)

		person_info_pub = rospy.Publisher('/cameraIP_publisher', Person1, queue_size=100)

		#设置循环的频率
		rate = rospy.Rate(10) 
		while not rospy.is_shutdown():
			# person_msg::Person类型的消息
			person_msg = Person1()
			flag = self.get_camera_ip()
		
			person_msg.camera_ip1=self.camera_ip1
			person_msg.camera_ip2=self.camera_ip2
			
			person_info_pub.publish(person_msg)
			self.camera_ip=[]
			rate.sleep()

if __name__ == '__main__':
    try:
        p = camera_ip()
        p.cameraIP_publisher()
    except rospy.ROSInterruptException:
        pass
